#include "remote_control.h"
#include "arm_behaviour.h"
#include "main.h"
//自定义遥控器的数据接收与处理放到这，返回遥控器结构体指针，代码规范（11111代码规范）与实现方式参考步兵遥控器数据接收。
RC_ctrl_t rc_ctrl;
extern UART_HandleTypeDef huart7;
#define FRAME_HEADER  0xFF    // 帧头
#define FRAME_TAIL    0xFE    // 帧尾
#define FRAME_SIZE    14       // 帧总长度（包含帧头和帧尾）
#define DATA_LENGTH   12       // 数据部分长度
void data2arm();
uint8_t rx_byte;           // 用于接收的单字节缓冲
uint8_t rx_buffer[14]; // 帧接收缓冲区
uint8_t frame_index = 0;   // 帧接收索引
uint8_t data_array[12]; // 存储控制器传来的数据
int16_t arm_ctr[6];

const RC_ctrl_t *get_remote_control_point(void)
{   
		data2arm();
    return &rc_ctrl;
}


void UART7_StartReceive(void)
{
    HAL_UART_Receive_IT(&huart7, &rx_byte, 1);
}



void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == UART7) // 确认是 UART7
    {
        if (frame_index == 0) // 等待帧头
        {
            if (rx_byte == FRAME_HEADER) // 检查帧头
            {
                rx_buffer[frame_index++] = rx_byte; // 存储帧头
//								HAL_UART_Transmit(&huart7, &rx_buffer[frame_index-1], 1, HAL_MAX_DELAY);
            }
        }
        else // 接收帧数据
        {
            rx_buffer[frame_index++] = rx_byte;
//						HAL_UART_Transmit(&huart7, &rx_buffer[frame_index-1], 1, HAL_MAX_DELAY);
					
            if (frame_index == FRAME_SIZE) // 接收到完整帧
            {
                if (rx_buffer[FRAME_SIZE - 1] == FRAME_TAIL) // 检查帧尾
                {
                    // 解析帧数据部分（中间12字节）
                    for (uint8_t i = 0; i < DATA_LENGTH; i++)
                    {
                        data_array[i] = rx_buffer[i + 1];
                    }
//										HAL_UART_Transmit(&huart7, &data_array[0], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[1], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[2], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[3], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[4], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[5], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[6], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[7], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[8], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[9], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[10], 1, HAL_MAX_DELAY);
//										HAL_UART_Transmit(&huart7, &data_array[11], 1, HAL_MAX_DELAY);
										
                }																																														

                frame_index = 0; // 重置索引，准备接收下一帧
            }
        }

        // 再次启动单字节中断接收
        HAL_UART_Receive_IT(&huart7, &rx_byte, 1);
    }
}

//原始数据转化为机械臂角度
void data2arm()
{
	arm_ctr[0]=(data_array[0] << 8) | data_array[1];
	arm_ctr[1]=(data_array[2] << 8) | data_array[3];
	arm_ctr[2]=(data_array[4] << 8) | data_array[5];
	arm_ctr[3]=(data_array[6] << 8) | data_array[7];
	arm_ctr[4]=(data_array[8] << 8) | data_array[9];
	arm_ctr[5]=(data_array[10] << 8) | data_array[11];
	

	rc_ctrl.joint.joint1_rad=(float)arm_ctr[0]*3.1415926/180;
	rc_ctrl.joint.joint2_rad=(float)arm_ctr[1]*3.1415926/180;
	rc_ctrl.joint.joint3_rad=(float)arm_ctr[2]*3.1415926/180;
	rc_ctrl.joint.joint4_rad=(float)arm_ctr[3]*3.1415926/180;
	rc_ctrl.joint.joint5_rad=(float)arm_ctr[4]*3.1415926/180;
	rc_ctrl.joint.joint6_rad=(float)arm_ctr[5]*3.1415926/180;
}
